#ifndef MOTOR_H
#define MOTOR_H

#define REDUCTION_RADIO 3591/187    //减速比
#define REDIUS 0.1   //半径，单位m
#define PI  3.14    //圆周率

typedef struct
{
    uint8_t type;
    float Kp;         // 比例系数
    float Ki;         // 积分系数
    float Kd;         // 微分系数
    float setpoint;   // 设定值
    float integral;   // 积分项
    float prev_error; //上上次误差
    float last_error;//上次误差
} PID_Controller;




extern PID_Controller pid_motor_position;//位置环初始化
extern PID_Controller pid_motor_speed;//速度环初始化
extern PID_Controller pid_mecanum[4];
void PID_position_init(PID_Controller *pid, float Kp, float Ki, float Kd, float setpoint);
float PID_position_calc(PID_Controller *pid, float measured_value, float setpoint);
float PID_plus_calc(PID_Controller *pid, float measured_value);
float PID_cascade_calc(PID_Controller *pid_inner,PID_Controller *pid_outer,int16_t outer_feedback,int16_t inner_feedback);
int16_t Angle_Control(int16_t Num,PID_Controller *speed,PID_Controller *position);
void Mec_Control(PID_Controller *Mec);

#endif